#include <arpa/inet.h>
#include <stdio.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <netdb.h>
#include <errno.h>
#include <string.h>
#include <unistd.h>
#include "robot_types.h"
#include "bhtpdvpls_structs.h"

#define DATA "POS1"
#define KOOR_PORT 7778
#define HEADERSIZE	112
#define IMAGESIZE		640*480*3

ssize_t                         /* Read "n" bytes from a descriptor. */
readn(int fd, void *vptr, size_t n)
{
    size_t  nleft;
    ssize_t nread;
    char   *ptr;

    ptr = (char*)vptr;
    nleft = n;
    while (nleft > 0) {
        if ( (nread = read(fd, ptr, nleft)) < 0) {
            if (errno == EINTR)
                nread = 0;      /* and call read() again */
            else
                return (-1);
        } else if (nread == 0)
            break;              /* EOF */

        nleft -= nread;
        ptr += nread;
    }
    return (n - nleft);         /* return >= 0 */
}

ssize_t                         /* Write "n" bytes to a descriptor. */
writen(int fd, const void *vptr, size_t n)
{
    size_t nleft;
    ssize_t nwritten;
    const char *ptr;

    ptr = (char*)vptr;
    nleft = n;
    while (nleft > 0) {
        if ( (nwritten = write(fd, ptr, nleft)) <= 0) {
            if (nwritten < 0 && errno == EINTR)
                nwritten = 0;   /* and call write() again */
            else
                return (-1);    /* error */
         }

         nleft -= nwritten;
         ptr += nwritten;
    }
    return (n);
}

int main( int argc, char **argv)
{
	ObjektListe ol;
   int sd_coorse;
   struct sockaddr_in co_server;
   char buf[20];
   int bufidx; //,readd;
	char buffer[IMAGESIZE];

   // Verbindung zum Koordinaten-Server herstellen
   sd_coorse = socket(AF_INET, SOCK_STREAM, 0);

   bzero(&co_server,sizeof(co_server));
   co_server.sin_family = AF_INET;
   co_server.sin_port = htons(KOOR_PORT);
   //pdv1 .101, pdv2 .102, pdv3 .103 ... pdv22 .122
   inet_pton(AF_INET, "141.64.25.101", &co_server.sin_addr);

   printf("Verbinde mit Koordinaten-Server...\n");
   if (connect(sd_coorse, (struct sockaddr *)&co_server, sizeof(co_server)) < 0)
   {
      printf("Kann keine Verbindung zum Koordinaten-Server aufbauen\n");
      perror("connect");
      return 1;
   }

   printf("c't-Bot Kommunikationsserver bereit\n");

   while ( 1 )
   {
      bufidx=0;
      memset(buf,0,sizeof(buf));
   
      write(sd_coorse, DATA, sizeof(DATA));
      readn(sd_coorse, &ol, sizeof(ol));
      readn(sd_coorse, buffer, HEADERSIZE);
      readn(sd_coorse, buffer, IMAGESIZE);
      printf("X: %4d, ", ol.kord[0].xkordinate_mm);
      printf("Y: %4d, ", ol.kord[0].ykordinate_mm);
      printf("Theta: %d\n", ol.kord[0].theta_grad);

		usleep(1000000);
   }

   close(sd_coorse);
   return 0;
}
